#include "Sensors.h"

uint8_t get_RS_sensor_data(uint16_t* data) {
	send_cmd_USART(READ_RS_SENSORS,0,0);
	return receive_cmd_results_USART((uint8_t*)data,10);
}

void get_IR_sensor_data(uint16_t* data) {
	turn_emitters_60_on();
	// Read sensor data
	for(unsigned int i = 0; i<4; i++) {
		data[i] = read_ADC(i);
	}
	turn_emitters_60_off();
}

void turn_emitters_60_on(){
	DDRC |= (1 << DDC5) | (1 << DDC6) | (1 << DDC7);
	DDRD |= (1 << DDD4) | (1 << DDD5) | (1 << DDD6) | (1 << DDD7);
	PORTC |= (1 << PORTC5) | (1 << PORTC6) | (1 << PORTC7);
	PORTD |= (1 << PORTD4) | (1 << PORTD5) | (1 << PORTD6) | (1 << PORTD7);
}

void turn_emitters_60_off(){
	PORTC &= ~((1 << PORTC5) | (1 << PORTC6) | (1 << PORTC7));
	PORTD &= ~((1 << PORTD5) | (1 << PORTD6) | (1 << PORTD7));
}

void turn_emitters_18_on(){
	DDRC |= (1 << DDC5) | (1 << DDC6) | (1 << DDC7);
	DDRD |= (1 << DDD5) | (1 << DDD6) | (1 << DDD7);  
	PORTC |= (1 << PORTC5) | (1 << PORTC6) | (1 << PORTC7);
	PORTD |= (1 << PORTD5) | (1 << PORTD6) | (1 << PORTD7);
}

void turn_emitters_18_off(){
	PORTC &= ~((1 << PORTC5) | (1 << PORTC6) | (1 << PORTC7));
	PORTD &= ~((1 << PORTD5) | (1 << PORTD6) | (1 << PORTD7));
}